
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       location.h
  * @author     baiyang
  * @date       2021-7-7
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>
#include <gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define LOCATION_ALT_MAX_M  83000   // maximum altitude (in meters) that can be fit into Location structure's alt field
/*----------------------------------typedef-----------------------------------*/
/// enumeration of possible altitude types
typedef enum
{
    ALT_FRAME_ABSOLUTE = 0,
    ALT_FRAME_ABOVE_HOME = 1,
    ALT_FRAME_ABOVE_ORIGIN = 2,
    ALT_FRAME_ABOVE_TERRAIN = 3,
}AltFrame;

/** @ 
  * @brief  位置点的一些属性
  */
typedef struct
{
    uint8_t relative_alt : 1;           // 1 if altitude is relative to home
    uint8_t loiter_ccw   : 1;           // 0 if clockwise, 1 if counter clockwise
    uint8_t terrain_alt  : 1;           // this altitude is above terrain
    uint8_t origin_alt   : 1;           // this altitude is above ekf origin
    uint8_t loiter_xtrack : 1;          // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
}Location_flags;

/** @ 
  * @brief  
  */
typedef struct
{
    int32_t alt_cm;      //原点的高度为海拔高度，单位：厘米
    int32_t lat;
    int32_t lng;

    struct {
        uint8_t alt_valid             : 1;           // 1 if altitude is valid
        uint8_t horizontal_valid      : 1;           // 0 if Effective in horizontal position
    } tflags;
}Origin_loc;

/** @ 
  * @brief  位置结构体
  */
typedef struct
{
    // note that mission storage only stores 24 bits of altitude (~ +/- 83km)
    int32_t alt;
    int32_t lat;
    int32_t lng;

    Location_flags       tflags;
}Location;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       判断位置结构体是不是零
  * @param[in]   tLocation  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_is_zero(Location *tLocation);

/**
  * @brief       清零位置结构体
  * @param[in]   tLocation  
  * @param[out]  
  * @retval      
  * @note        
  */
void location_zero(Location *tLocation);

/**
  * @brief       设置位置点高度
  * @param[in]   tLocation  
  * @param[in]   alt_cm  
  * @param[in]   frame  
  * @param[out]  
  * @retval      
  * @note        
  */
void location_set_alt_cm(Location *tLocation, int32_t alt_cm, AltFrame frame);

/**
  * @brief       设置位置点经纬度和高度
  * @param[in]   tLocation  
  * @param[in]   alt_cm  
  * @param[in]   frame  
  * @param[out]  
  * @retval      
  * @note        
  */
void location_from_lat_lon_alt(Location *tLocation, int32_t latitude, int32_t longitude, int32_t alt_in_cm, AltFrame frame);

/**
  * @brief       获取高度的坐标系
  * @param[in]   tLocation  
  * @param[out]  
  * @retval      
  * @note        
  */
AltFrame location_get_alt_frame(const Location *tLocation);

/**
  * @brief       获取期望坐标下的高度
  * @param[in]   tLocation  
  * @param[in]   desired_frame  
  * @param[in]   ret_alt_cm  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_get_alt_cm(const Location *tLocation, AltFrame desired_frame, int32_t *ret_alt_cm);

/**
  * @brief       将高度值转换到新的坐标系下
  * @param[in]   tLocation  
  * @param[in]   desired_frame  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_change_alt_frame(Location *tLocation, AltFrame desired_frame);

/**
  * @brief       返回北东坐标系下的，以tLocation为原点，loc2为目标点的矢量
  * @param[in]   tLocation  
  * @param[in]   vec_ne  
  * @param[out]  
  * @retval      
  * @note        单位：厘米
  */
bool location_get_vector_xy_from_origin_ne(const Location *tLocation, Vector2f_t *vec_ne);

/**
  * @brief       返回北东天坐标系下的，以tLocation为原点，loc2为目标点的矢量
  * @param[in]   tLocation  
  * @param[in]   vec_neu  
  * @param[out]  
  * @retval      
  * @note        单位：厘米
  */
bool location_get_vector_from_origin_neu(Location *tLocation, Vector3f_t *vec_neu);

/**
  * @brief       返回两个位置点之间的距离
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        单位：米
  */
float location_get_distance(Location *tLocation, const Location *loc2);

/**
  * @brief       返回北东坐标系下的，以tLocation为原点，loc2为目标点的矢量
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        
  */
Vector2f_t location_get_distance_ne(const Location *tLocation, const Location *loc2);

/**
  * @brief       返回北东地坐标系下的，以tLocation为原点，loc2为目标点的矢量
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        
  */
Vector3f_t location_get_distance_ned(const Location *tLocation, const Location *loc2);

/**
  * @brief       根据北和东给定距离（以米为单位），推算纬度/经度
  * @param[in]   tLocation  
  * @param[in]   ofs_north  
  * @param[in]   ofs_east  
  * @param[out]  
  * @retval      
  * @note        
  */
void location_offset(Location *tLocation, float ofs_north, float ofs_east);

/**
  * @brief       在给定方位角和距离的情况下外推纬度/经度
  * @param[in]   tLocation  
  * @param[in]   bearing  
  * @param[in]   distance  
  * @param[out]  
  * @retval      
  * @note        请注意，此功能在100m的距离处精确到大约1mm。
  *              此功能的优点是可以在相对位置工作，因此即使在处理小距离和浮点数时也可以保持精度
  */
void location_offset_bearing(Location *tLocation, float bearing, float distance);

/**
  * @brief       根据给定的方位，螺距和距离推断纬度/经度
  * @param[in]   tLocation  
  * @param[in]   bearing  
  * @param[in]   pitch  
  * @param[in]   distance  
  * @param[out]  
  * @retval      
  * @note        
  */
void location_offset_bearing_and_pitch(Location *tLocation, float bearing, float pitch, float distance);

/**
  * @brief       检测纬度是否是有效值
  * @param[in]   lat  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_check_lat(int32_t lat);

/**
  * @brief       检测经度是否是有效值
  * @param[in]   lng  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_check_lng(int32_t lng);

/**
  * @brief       当lat和lng在范围内时，返回true
  * @param[in]   tLocation  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_check_lat_lng(Location *tLocation);

/**
  * @brief       用有用的数据转换无效的航点。如果位置点被更改，则返回true
  * @param[in]   tLocation  
  * @param[in]   defaultLoc  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_sanitize(Location *tLocation, const Location *defaultLoc);

/**
  * @brief       点loc2在点tLocation北偏东的角度
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        单位：度*100
  */
int32_t location_get_bearing_to(Location *tLocation, const Location *loc2);

/**
  * @brief       如果经纬度相同，则返回true
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_same_lat_lon_as(Location *tLocation, const Location *loc2);

/**
  * @brief       返回点tLocation在线段point1<->point2投影，
  *              与点point1的长度和point1<->point2长度的比例
  * @param[in]   tLocation  
  * @param[in]   point1  
  * @param[in]   point2  
  * @param[out]  
  * @retval      
  * @note        point1为线段起点，point2为线段终点
  */
float location_line_path_proportion(Location *tLocation, const Location *point1, const Location *point2);

/**
  * @brief       如果点tLocation在线段point1->point2的投影，
  *              落在线段point1->point2外，且在point2一侧,则返回true
  * @param[in]   tLocation  
  * @param[in]   point1  
  * @param[in]   point2  
  * @param[out]  
  * @retval      
  * @note        point1为线段起点，point2为线段终点
  */
bool location_past_interval_finish_line(Location *tLocation, const Location *point1, const Location *point2);

/**
  * @brief       设置原点经纬度及高度
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void location_set_origin(Origin_loc origin);

/**
  * @brief       获取原点信息
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
const Origin_loc* location_get_origin(void);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



